/*
 * Copyright (c) 2018-2023, Texas Instruments Incorporated
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * *  Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *
 * *  Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * *  Neither the name of Texas Instruments Incorporated nor the names of
 *    its contributors may be used to endorse or promote products derived
 *    from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
 * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
 * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
 * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
 * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

/*
 *  ======== i2ctmp.c ========
 */
#include <stdint.h>
#include <stddef.h>
#include <unistd.h>

/* Driver Header files */
#include <ti/drivers/GPIO.h>
#include <ti/drivers/I2C.h>
#include <ti/display/Display.h>

/* Driver configuration */
#include "ti_drivers_config.h"
#include "QMC5883.h"
//#include "empty.h"
#include <pthread.h>

extern Display_Handle display;

#define QMC5883STACKSIZE 640

I2C_Handle i2c;
I2C_Params i2cParams;

//static uint8_t targetAddress;

static void i2cErrorHandler(I2C_Transaction *transaction, Display_Handle display);

void qmc5883_user_delay_us(uint32_t period_us)
{
    /* Wait for a period amount of microseconds. */
    //esp_rom_delay_us(period_us);
    usleep(period_us);
}

int8_t qmc5883_user_i2c_reg_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t length, uint8_t dev_addr)
{

    // Write to registers using I2C. Return 0 for a successful execution.
    I2C_Transaction i2cTransaction;
    int ret,i=0;
    uint8_t rxBuffer[2];
     //dev_addr = *(uint8_t*)intf_ptr;
    uint8_t write_buf[length+1];
    write_buf[0] = reg_addr;
    for(i=1;i<length+1;i++)
    {
        write_buf[i] = reg_data[i-1];
    }
     /* Common I2C transaction setup */
    i2cTransaction.writeBuf   = write_buf;
    i2cTransaction.writeCount = sizeof(write_buf);
    i2cTransaction.readBuf    = rxBuffer;
    i2cTransaction.readCount  = 0;
    i2cTransaction.targetAddress = dev_addr;

    ret = I2C_transfer(i2c, &i2cTransaction);
    return ret; 
}

int8_t qmc5883_user_i2c_reg_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t length, uint8_t dev_addr)
{
    //dev_addr = *(uint8_t*)intf_ptr;
    /* Read from registers using I2C. Return 0 for a successful execution. */
    I2C_Transaction i2cTransaction;
     /* Common I2C transaction setup */
    i2cTransaction.writeBuf   = reg_data;
    i2cTransaction.writeCount = 1;
    i2cTransaction.readBuf    = reg_data;
    i2cTransaction.readCount  = length;
    i2cTransaction.targetAddress = dev_addr;
    return I2C_transfer(i2c, &i2cTransaction);
}

uint8_t qmc5883_init(void)
{
    uint8_t ret=0;
    qmc5883_user_i2c_reg_read(QMC5883_CHIP_ID,&ret,1,QMC5883_I2C_ADDRESS);
    Display_printf(display, 0, 0,"chip id %d \n",ret);
    qmc5883_user_delay_us(1000);

    uint8_t dat = 0x06;
    ret = qmc5883_user_i2c_reg_write(QMC5883_REG_FRB,&dat,1,QMC5883_I2C_ADDRESS);
    Display_printf(display, 0, 0,"FBR retrun %d \n",ret);

    QMC5883_REG1_MODE reg1 = {
        .SET_RESET_MODE = QMC5883_SETRESET_SETON,
        .RNG = QMC5883_RNG_16G,
        .SELF_TEST = QMC5883_SELFTEST_DISABLE,
        .SOFT_RST = QMC5883_SOFTRESET_DISABLE
    };
    ret = qmc5883_user_i2c_reg_write(QMC5883_REG1_CONFIG,&reg1.configs,1,QMC5883_I2C_ADDRESS);
    Display_printf(display, 0, 0,"reg1 config %d retrun %d \n",reg1.configs,ret);

    QMC5883_REG0_MODE reg0 ={
        .mode = QMC5883_MODE_Continuous,
        .ODR = QMC5883_ODR_200HZ,
        .OSR1 = QMC5883_OSR1_8,
        .OSR2 = QMC5883_OSR2_8
    };
    ret = qmc5883_user_i2c_reg_write(QMC5883_REG0_CONFIG,&reg0.configs,1,QMC5883_I2C_ADDRESS);
    Display_printf(display, 0, 0,"reg0 config %d retrun %d \n",reg0.configs,ret);
    
    return ret;
}

void QMC5883_get_data(void)
{
    uint8_t data[6] = {0};
    qmc5883_user_i2c_reg_read(QMC5883_REG_XDATA_LSB,data,6,QMC5883_I2C_ADDRESS);
    int16_t datax = ((int16_t)data[0]) | ((int16_t)(data[1] << 8));  
    int16_t datay = ((int16_t)data[2]) | ((int16_t)(data[3] << 8));  
    int16_t dataz = ((int16_t)data[4]) | ((int16_t)(data[5] << 8));  

    Display_printf(display, 0, 0,"X : %d; Y : %d; Z : %d\n", datax, datay, dataz);
}

/*
 *  ======== QMC5883Thread ========
 */
void *QMC5883Thread(void *arg0)
{
    uint16_t sample;
    int16_t temperature;
    uint8_t txBuffer[1];
    uint8_t rxBuffer[2];
    int8_t i;
    I2C_Transaction i2cTransaction;
    /* Create I2C for usage */
    I2C_Params_init(&i2cParams);
    i2cParams.bitRate = I2C_100kHz;
    i2c               = I2C_open(CONFIG_I2C_QMC5883, &i2cParams);
    if (i2c == NULL)
    {
        Display_printf(display, 0, 0, "Error Initializing I2C\n");
        while (1) {}
    }
    else
    {
        Display_printf(display, 0, 0, "I2C Initialized!\n");
    }

    qmc5883_init();
    for(;;)
    {
        QMC5883_get_data();
        usleep(5000);
    }
    

    I2C_close(i2c);
    Display_printf(display, 0, 0, "I2C closed!");

    return (NULL);
}

void QMC5883_ThreadStart(void)
{
    pthread_t thread;
    pthread_attr_t attrs;
    struct sched_param priParam;
    int retc;

    pthread_attr_init(&attrs);

    /* Set priority, detach state, and stack size attributes */
    priParam.sched_priority = 2;
    retc                    = pthread_attr_setschedparam(&attrs, &priParam);
    retc |= pthread_attr_setdetachstate(&attrs, PTHREAD_CREATE_DETACHED);
    retc |= pthread_attr_setstacksize(&attrs, QMC5883STACKSIZE);
    if (retc != 0)
    {
        /* failed to set attributes */
        while (1) {}
    }

    retc = pthread_create(&thread, &attrs, QMC5883Thread, NULL);
    if (retc != 0)
    {
        /* pthread_create() failed */
        while (1) {}
    }
}

/*
 *  ======== i2cErrorHandler ========
 */
static void i2cErrorHandler(I2C_Transaction *transaction, Display_Handle display)
{
    switch (transaction->status)
    {
        case I2C_STATUS_TIMEOUT:
            Display_printf(display, 0, 0, "I2C transaction timed out!");
            break;
        case I2C_STATUS_CLOCK_TIMEOUT:
            Display_printf(display, 0, 0, "I2C serial clock line timed out!");
            break;
        case I2C_STATUS_ADDR_NACK:
            Display_printf(display,
                           0,
                           0,
                           "I2C target address 0x%x not"
                           " acknowledged!",
                           transaction->targetAddress);
            break;
        case I2C_STATUS_DATA_NACK:
            Display_printf(display, 0, 0, "I2C data byte not acknowledged!");
            break;
        case I2C_STATUS_ARB_LOST:
            Display_printf(display, 0, 0, "I2C arbitration to another controller!");
            break;
        case I2C_STATUS_INCOMPLETE:
            Display_printf(display, 0, 0, "I2C transaction returned before completion!");
            break;
        case I2C_STATUS_BUS_BUSY:
            Display_printf(display, 0, 0, "I2C bus is already in use!");
            break;
        case I2C_STATUS_CANCEL:
            Display_printf(display, 0, 0, "I2C transaction cancelled!");
            break;
        case I2C_STATUS_INVALID_TRANS:
            Display_printf(display, 0, 0, "I2C transaction invalid!");
            break;
        case I2C_STATUS_ERROR:
            Display_printf(display, 0, 0, "I2C generic error!");
            break;
        default:
            Display_printf(display, 0, 0, "I2C undefined error case!");
            break;
    }
}
